// c2prog.ino
//
// Flash Ardupilot bootloader to SiK radios via C2 interface
// using an Arduino.
//
// Created by Iain Guilliard on 28/09/2019.
//
// Based on the AVR code in https://github.com/x893/C2.Flash
// Which in turn is based on the code examples provided in
// the orginal Si Labs application note, AN127 Rev. 1.1

// HM-TRP C2 interface
// -------------------
//
// The HopeRF HM-TRP board conveniently provides pads for programming
// the Si1000.  These are the four 2mm-spaced holes next to the LEDs,
// near the edge of the board ('o' in this picture).
//
// +-----------~~
// | O #:#:
// | O = +------+
// | O = |      |
// | O   |      |
// | O o +------+  <- C2CK
// | O o     +-+   <- C2D
// | O o # # | |   <- VDD_MCU
// | O o     +-+   <- GND
// +---^-------~~
//     |
//  This row of 4 holes.
//
// NOTE! Si1000 VDD_MCU is not 5V tolerant! Only connect VDD_MCU to +3.3V
// I/O pins including C2CK and C2D are 5V tolerant


void start_mesg() { Serial.print(F(R"(
                c2prog
                
Flash Ardupilot bootloader to SiK radios via C2 interface:
  Arduino        HM-TRP
 ~~-------+    +-----------~~
          |    | O #:#:
          |    | O = +------+
          |    | O = |      |
          |    | O   |      |
      D12 | -----> o +------+  <- C2CK
      D11 | -----> o     +-+   <- C2D
     +3V3 | -----> o # # | |   <- VDD_MCU
      GND | -----> o     +-+   <- GND
 ~~-------+    +-----------~~
  Warning! VDD_MCU is NOT 5V tolerant.
 
)"));}

void *keys_mesg() { Serial.print(F(R"(
Keys:
    4 : Set the bootloader version to 433 (default)
    8 : Set the bootloader version to 868
    9 : Set the bootloader version to 915
    D : Get Device ID
    E : Erase Flash
    B : Burn bootloader
    V : Verify bootloader
    W : Write Flash: WAAAALLbb..bb
    R : Read Flash: RAAAALL
    
        AAAA = 16 bit start address in hex
        LL = length in hex, 0-256 bytes
        bb..bb = LL hex bytes to write
)"));}

void instructions_mesg() { Serial.print(F(R"(
Instructions:
  1) Connect Ardiuno to Sik board C2 interface as shown in the diagram.
  2) Check Device ID. e.g. 1600 = Si100x, rev. A, 0000 = not connected.
  3) Erase Flash.
  4) Burn bootloader.
  5) Verify bootloader (lock byte is not flashed, so ignor failure at address 0xFBFF).
)"));}

// Ardiuno Pins to C2 interface
// ----------------------------
//
// C2 Connection   Arduino   Uno/Nano/Mini   Mega2560
//                           (ATmega328P)    (ATmega2560)
// ------------------------------------------------------
// C2CK            D12       PB4              PB6     
// C2D             D11       PB3              PB5
// VDD_MCU         +3V3
// GND             GND
// ---------------         
// Ardiuno LED     D13       PB5              PB7
// ------------------------------------------------------

// Set Arduino pins to use 
#define ARDUINO_C2D_PIN 2
#define ARDUINO_C2K_PIN 3
#define ARDUINO_LED_PIN LED_BUILTIN

// Map Arduino pins to ATmega328P or ATmega2560 port B pins
// Use low level access for speed
#define C2CK_BIT  digital_pin_to_bit_mask_PGM[ARDUINO_C2K_PIN] // _BV(6) // mega2560 D12
#define C2CK_PORT PORTD
#define C2CK_PIN  PIND
#define C2CK_DDR  DDRD

#define C2D_BIT   digital_pin_to_bit_mask_PGM[ARDUINO_C2D_PIN] // _BV(5) // mega2560 D11
#define C2D_PORT  PORTD
#define C2D_PIN   PIND
#define C2D_DDR   DDRD

// Comment next line to disable LED
#define LED_BIT   digital_pin_to_bit_mask_PGM[ARDUINO_LED_PIN] // _BV(7) // mega2560 D13
#define LED_PORT  PORTB
#define LED_PIN   PINB
#define LED_DDR   DDRB

#ifdef LED_BIT
  #define LED_ON()  (LED_PORT |= LED_BIT)
  #define LED_OFF() (LED_PORT &= ~LED_BIT)
#else
  #define LED_ON()
  #define LED_OFF()
#endif

// DeviceID definitions
#define C8051F32X_DEVICEID    0x09
#define SI100X_DEVICEID     0x16

// C2 Host command
#define C2_CONNECT_TARGET       0x20
#define C2_DISCONNECT_TARGET    0x21
#define C2_DEVICE_ID            0x22
#define C2_UNIQUE_DEVICE_ID     0x23
#define C2_TARGET_GO            0x24
#define C2_TARGET_HALT          0x25

#define C2_READ_SFR             0x28
#define C2_WRITE_SFR            0x29
#define C2_READ_RAM             0x2A
#define C2_WRITE_RAM            0x2B
#define C2_READ_FLASH           0x2E
#define C2_WRITE_FLASH          0x2F
#define C2_ERASE_PAGE           0x30

#define C2_ADDRESSWRITE         0x38
#define C2_ADDRESSREAD          0x39
#define C2_DATAWRITE            0x3A
#define C2_DATAREAD             0x3B

#define C2_ERASE_FLASH_03       0x3C
#define C2_ERASE_FLASH_04       0x3D
#define C2_READ_XRAM            0x3E
#define C2_WRITE_XRAM           0x3F

#define GET_FIRMWARE_VERSION  0x44
#define SET_FPDAT_ADDRESS   0x45
#define GET_FPDAT_ADDRESS   0x46

// C2 status return codes
#define COMMAND_INVALID     0x00
#define COMMAND_NO_CONNECT    0x01
#define COMMAND_FAILED      0x02
#define COMMAND_TIMEOUT     0x04
#define COMMAND_BAD_DATA    0x05
#define COMMAND_OK        0x0D
#define COMMAND_READ_01     0x81
#define COMMAND_READ_02     0x82
#define COMMAND_READ_03     0x83
#define COMMAND_READ_04     0x84

// C2 FP Command
#define C2_FP_ERASE_DEVICE    0x03
#define C2_FP_READ_FLASH    0x06
#define C2_FP_WRITE_FLASH   0x07
#define C2_FP_ERASE_SECTOR    0x08

#define C2_FP_READ_XRAM     0x0E
#define C2_FP_WRITE_XRAM    0x0F

#define C2ADD_OUTREADY  0x01
#define C2ADD_INBUSY  0x02

#define C2ADD_DEVICEID  0
#define C2ADD_REVISION  1
#define C2ADD_FPCTL   2
byte  C2ADD_FPDAT   = 0xAD;

byte DeviceID   = 0;
byte C2_CONNECTED = 0;
byte LEDS_STATE   = 0;
byte C2_RESPONSE  = 0;

byte cmdPackage[140];
byte cmdPackageLength;
byte cmdPackageGet;

#define STRINGIFY(...) #__VA_ARGS__

const char bootloader_hex_433[] PROGMEM  = STRINGIFY(
:06000000020311020403DB
:03000B0002040BE1
:03001300020413D1
:03001B0002041BC1
:03002300020423B1
:03002B0002042BA1
:0300330002043391
:03003B0002043B81
:0300430002044371
:03004B0002044B61
:0300530002045351
:03005B0002045B41
:0300630002046331
:03006B0002046B21
:0300730002047311
:03007B0002047B01
:03008300020483F1
:03008B0002048BE1
:03009300020493D1
:03009B0002049BC1
:0300A3000204A3B1
:0300AB000204ABA1
:0300B3000204B391
:0803110075814D1200BB80FE56
:2000BB001202AAE5EFF54830E10375480212F800E58224FFE433F54AD2827549007F00BF49
:2000DB00FF00500820820205490F80F3D294E54820E617E54A601374382549400D90FBFEF8
:2000FB00E493F5C475C30212040012010A80FBD294120323AF82C294BF21028017BF22024C
:20011B008012BF2302800DBF26028008BF29028003BF310FC007120323AE82D007BE200200
:20013B00800122BF2100500122EF24CE500122EF24DFFF240A83F582EF241583F583E473C7
:20015B009A7D909CBFE90060969D9D9D9D9D9D979A020101010101020201020202020202AC
:20017B00020275823E12031990FBFEE493F58212031902029E12F83802029E12F86A02025A
:20019B009E120323AF828F4B754C00120323AF828F06E4424BEE424C120323AF82BF20036C
:2001BB0002029E22120323AF82C007120323AE82D007BE2002800122854B82854C83054B78
:2001DB00E4B54B02054C8F4D12F87A02029E854B82854C83054BE4B54B02054C12F8A612DC
:2001FB00031902029E120323E582FF24BF5001227E00C3EE9F5019EE2408F9C007C006C09B
:20021B0001120323E582D001D006D007F70E80E2C007120323AE82D007BE20667E00C3EEC5
:20023B009F505C854B82854C83054BE4B54B02054CEE2408F9874DC007C00612F87AD0065D
:20025B00D0070E80D9120323AF82C007120323AE82D007BE202C7E00C3EE9F5022854B823A
:20027B00854C83054BE4B54B02054CC007C00612F8A6120319D006D0070E80DC43EF1002C2
:20029B00029E2275821212031975821002031975A700C2AF53D9BF75B64075B28F75A9006E
:2002BB00758840758920758E08758D9675981275FF807E5E7F01EE24FFFCEF34FFFD8C06F8
:2002DB008D07EC4D70F075EF0675A41075A70F75A41075A70075E10143D40E43D53043A428
:1602FB008043A53D75A70F43A50D75A70075E401C28875E34022AE
:20F8000090F7FEE493FFBF3D0B90F7FFE493FFBFC20280047F0080027F018F8222AF837489
:20F82000FC2F500574082F5004758200227582012275B7A575B7F1227E007FF4C007C00688
:20F8400012F831D006D007758F038E828F8374FFF0758F008E048F05ED24FCFD8C068D07DA
:20F860008E048F0574FC2D40D32212F831758F0790000074FFF0758F0022AE82AF83C00708
:20F88000C00612F81DE582D006D0076018C007C00612F831D006D007758F018E828F83E56E
:0BF8A0004DF0758F0022E493F58222EA
:02FBFE0043FEC4
:13031900AF8210990280FB8F992210980280FB8599822249
:00000001FF

);

const char bootloader_hex_868[] PROGMEM  = STRINGIFY(
:06000000020311020403DB
:03000B0002040BE1
:03001300020413D1
:03001B0002041BC1
:03002300020423B1
:03002B0002042BA1
:0300330002043391
:03003B0002043B81
:0300430002044371
:03004B0002044B61
:0300530002045351
:03005B0002045B41
:0300630002046331
:03006B0002046B21
:0300730002047311
:03007B0002047B01
:03008300020483F1
:03008B0002048BE1
:03009300020493D1
:03009B0002049BC1
:0300A3000204A3B1
:0300AB000204ABA1
:0300B3000204B391
:0803110075814D1200BB80FE56
:2000BB001202AAE5EFF54830E10375480212F800E58224FFE433F54AD2827549007F00BF49
:2000DB00FF00500820820205490F80F3D294E54820E617E54A601374382549400D90FBFEF8
:2000FB00E493F5C475C30212040012010A80FBD294120323AF82C294BF21028017BF22024C
:20011B008012BF2302800DBF26028008BF29028003BF310FC007120323AE82D007BE200200
:20013B00800122BF2100500122EF24CE500122EF24DFFF240A83F582EF241583F583E473C7
:20015B009A7D909CBFE90060969D9D9D9D9D9D979A020101010101020201020202020202AC
:20017B00020275823E12031990FBFEE493F58212031902029E12F83802029E12F86A02025A
:20019B009E120323AF828F4B754C00120323AF828F06E4424BEE424C120323AF82BF20036C
:2001BB0002029E22120323AF82C007120323AE82D007BE2002800122854B82854C83054B78
:2001DB00E4B54B02054C8F4D12F87A02029E854B82854C83054BE4B54B02054C12F8A612DC
:2001FB00031902029E120323E582FF24BF5001227E00C3EE9F5019EE2408F9C007C006C09B
:20021B0001120323E582D001D006D007F70E80E2C007120323AE82D007BE20667E00C3EEC5
:20023B009F505C854B82854C83054BE4B54B02054CEE2408F9874DC007C00612F87AD0065D
:20025B00D0070E80D9120323AF82C007120323AE82D007BE202C7E00C3EE9F5022854B823A
:20027B00854C83054BE4B54B02054CC007C00612F8A6120319D006D0070E80DC43EF1002C2
:20029B00029E2275821212031975821002031975A700C2AF53D9BF75B64075B28F75A9006E
:2002BB00758840758920758E08758D9675981275FF807E5E7F01EE24FFFCEF34FFFD8C06F8
:2002DB008D07EC4D70F075EF0675A41075A70F75A41075A70075E10143D40E43D53043A428
:1602FB008043A53D75A70F43A50D75A70075E401C28875E34022AE
:20F8000090F7FEE493FFBF3D0B90F7FFE493FFBFC20280047F0080027F018F8222AF837489
:20F82000FC2F500574082F5004758200227582012275B7A575B7F1227E007FF4C007C00688
:20F8400012F831D006D007758F038E828F8374FFF0758F008E048F05ED24FCFD8C068D07DA
:20F860008E048F0574FC2D40D32212F831758F0790000074FFF0758F0022AE82AF83C00708
:20F88000C00612F81DE582D006D0076018C007C00612F831D006D007758F018E828F83E56E
:0BF8A0004DF0758F0022E493F58222EA
:02FBFE0086FE81
:13031900AF8210990280FB8F992210980280FB8599822249
:00000001FF

);

const char bootloader_hex_915[] PROGMEM  = STRINGIFY(
:06000000020311020403DB
:03000B0002040BE1
:03001300020413D1
:03001B0002041BC1
:03002300020423B1
:03002B0002042BA1
:0300330002043391
:03003B0002043B81
:0300430002044371
:03004B0002044B61
:0300530002045351
:03005B0002045B41
:0300630002046331
:03006B0002046B21
:0300730002047311
:03007B0002047B01
:03008300020483F1
:03008B0002048BE1
:03009300020493D1
:03009B0002049BC1
:0300A3000204A3B1
:0300AB000204ABA1
:0300B3000204B391
:0803110075814D1200BB80FE56
:2000BB001202AAE5EFF54830E10375480212F800E58224FFE433F54AD2827549007F00BF49
:2000DB00FF00500820820205490F80F3D294E54820E617E54A601374382549400D90FBFEF8
:2000FB00E493F5C475C30212040012010A80FBD294120323AF82C294BF21028017BF22024C
:20011B008012BF2302800DBF26028008BF29028003BF310FC007120323AE82D007BE200200
:20013B00800122BF2100500122EF24CE500122EF24DFFF240A83F582EF241583F583E473C7
:20015B009A7D909CBFE90060969D9D9D9D9D9D979A020101010101020201020202020202AC
:20017B00020275823E12031990FBFEE493F58212031902029E12F83802029E12F86A02025A
:20019B009E120323AF828F4B754C00120323AF828F06E4424BEE424C120323AF82BF20036C
:2001BB0002029E22120323AF82C007120323AE82D007BE2002800122854B82854C83054B78
:2001DB00E4B54B02054C8F4D12F87A02029E854B82854C83054BE4B54B02054C12F8A612DC
:2001FB00031902029E120323E582FF24BF5001227E00C3EE9F5019EE2408F9C007C006C09B
:20021B0001120323E582D001D006D007F70E80E2C007120323AE82D007BE20667E00C3EEC5
:20023B009F505C854B82854C83054BE4B54B02054CEE2408F9874DC007C00612F87AD0065D
:20025B00D0070E80D9120323AF82C007120323AE82D007BE202C7E00C3EE9F5022854B823A
:20027B00854C83054BE4B54B02054CC007C00612F8A6120319D006D0070E80DC43EF1002C2
:20029B00029E2275821212031975821002031975A700C2AF53D9BF75B64075B28F75A9006E
:2002BB00758840758920758E08758D9675981275FF807E5E7F01EE24FFFCEF34FFFD8C06F8
:2002DB008D07EC4D70F075EF0675A41075A70F75A41075A70075E10143D40E43D53043A428
:1602FB008043A53D75A70F43A50D75A70075E401C28875E34022AE
:20F8000090F7FEE493FFBF3D0B90F7FFE493FFBFC20280047F0080027F018F8222AF837489
:20F82000FC2F500574082F5004758200227582012275B7A575B7F1227E007FF4C007C00688
:20F8400012F831D006D007758F038E828F8374FFF0758F008E048F05ED24FCFD8C068D07DA
:20F860008E048F0574FC2D40D32212F831758F0790000074FFF0758F0022AE82AF83C00708
:20F88000C00612F81DE582D006D0076018C007C00612F831D006D007758F018E828F83E56E
:0BF8A0004DF0758F0022E493F58222EA
:02FBFE0091FE76
:13031900AF8210990280FB8F992210980280FB8599822249
:00000001FF

);

char * bootloader_hex = bootloader_hex_433;

byte verify_buffer[140];

void setup()
{
  LED_DDR |= LED_BIT;
  LED_OFF();

  C2CK_DriverOn();
  C2D_DriverOff();
  delay(10);
  
  Serial.begin(115200);
  start_mesg();
  keys_mesg();
  instructions_mesg();
  Serial.print("Device ID: ");
  C2_Reset();
  C2_Device_ID();

  Serial.println();

  C2_Connect_Target();
}

void loop()
{
  delay(1);
  byte c = toupper(Serial.read());
  //byte command = char2hex(c);

  if (c == 'D') {
    
    Serial.print("Device ID: ");
    C2_Reset();
    C2_Device_ID();
    //PutByte(DeviceID);
    Serial.println();
    
  } else if (c == 'R') {
    
    byte ah = Serial.read();
    byte al = Serial.read();
    byte addr_hi = (char2hex(ah) << 4) + char2hex(al);
    ah = Serial.read();
    al = Serial.read();
    byte addr_lo = (char2hex(ah) << 4) + char2hex(al);
    ah = Serial.read();
    al = Serial.read();
    byte len = (char2hex(ah) << 4) + char2hex(al);
    cmdPackage[0] = addr_lo; //addr lo
    cmdPackage[1] = addr_hi; // addr hi
    cmdPackage[2] = len; // length, 0-256 bytes
  
    cmdPackageLength=3;
    cmdPackageGet = 0;

    C2_Connect_Target();
    C2_Read_Memory(C2_FP_READ_FLASH);
    Serial.println();
    
  } else if (c == 'W') {
    
    byte ah = Serial.read();
    byte al = Serial.read();
    byte addr_hi = (char2hex(ah) << 4) + char2hex(al);
    ah = Serial.read();
    al = Serial.read();
    byte addr_lo = (char2hex(ah) << 4) + char2hex(al);
    ah = Serial.read();
    al = Serial.read();
    byte len = (char2hex(ah) << 4) + char2hex(al);
    cmdPackage[0] = addr_lo; //addr lo; //addr lo
    cmdPackage[1] = addr_hi; //addr_hi; // addr hi
    cmdPackage[2] = len; // length, 0-256 bytes
    byte b;

    for (byte i=0; i<len; i++) {

      ah = Serial.read();
      al = Serial.read();
      b = (char2hex(ah) << 4) + char2hex(al);
      cmdPackage[3 + i] = b; // data
      
    }

    cmdPackageLength = 3 + len;
    cmdPackageGet = 0;
  
    C2_Connect_Target();
    C2_Write_Memory(C2_FP_WRITE_FLASH);
    PutByte(C2_RESPONSE);

  } else if (c == 'B') {
    
    byte lock_byte = 0;
    Serial.println("\nBurning hex data to flash:");
    char *data_ptr = bootloader_hex;
    byte pass = 1;
    
    for(;;) {
      
        byte type = char2byte(data_ptr + 7);
        if (pgm_read_byte(data_ptr) != ':') {
          
            pass = 0;
            Serial.println("\nERROR: Hex file format\n");
            break;
            
        }

        if (type != 0) break;
        byte len = char2byte(data_ptr + 1);
        byte addr_hi = char2byte(data_ptr + 3);
        byte addr_lo = char2byte(data_ptr + 5);
        data_ptr += 9;
        byte chksum = type + len + addr_hi + addr_lo;
        
        cmdPackage[0] = addr_lo;
        cmdPackage[1] = addr_hi;
        cmdPackage[2] = len;
        
        for(int i=0;i < len;i++) {
          
            cmdPackage[3+i] = char2byte(data_ptr);
            chksum += cmdPackage[3+i];

            if (addr_hi == 0xfb && addr_lo + i == 0xff) {
               //Serial.println("lock byte detected");
               lock_byte = cmdPackage[3+i];
               cmdPackage[3+i] = 0xff;
            }
            data_ptr += 2;
        }
        chksum = 0x100 - chksum;
        if (char2byte(data_ptr) != chksum) {
            pass = 0;
            Serial.print("\nERROR: checksum fail: ");
            PutByte(char2byte(data_ptr));
            Serial.print(" != ");
            PutByte(chksum);
            Serial.println();
            break;
        }
        
        //Serial.println();
        cmdPackageLength = 3 + len;
        cmdPackageGet = 0;
        C2_Connect_Target();
        C2_Write_Memory(C2_FP_WRITE_FLASH);
        if (C2_RESPONSE != 0x0d) {
            pass = 0;
            Serial.print("W C2_RESPONSE: ");
            PutByte(C2_RESPONSE);
            Serial.println();
        }
        cmdPackageLength = 3;
        cmdPackageGet = 0;
        //C2_Connect_Target();
        C2_Read_Memory_to_buffer(C2_FP_READ_FLASH, verify_buffer);
        if (C2_RESPONSE != 0x0d) {
            pass = 0;
            Serial.print("R C2_RESPONSE: ");
            PutByte(C2_RESPONSE);
            Serial.println();
        }
        Serial.print(".");
        for (byte i=0; i<len; i++) {
          //PutByte(verify_buffer[i]);
          if (verify_buffer[i] != cmdPackage[3+i]) {
            pass = 0;
            Serial.println();
            PutByte(addr_hi);
            PutByte(addr_lo);
            Serial.print(" Failed at byte: ");
            PutByte(i);
            Serial.print(", Flash byte ");
            PutByte(verify_buffer[i]);
            Serial.print(" != ihex byte: ");
            PutByte(cmdPackage[3+i]);
            Serial.println();
            break;
          } else {
            
          }
        }
        ///Serial.println();
        
        data_ptr += 3;
    }
    Serial.print("\nBurning ihex data to flash: ");
    if (pass == 1) {
      Serial.println("passed.");
    } else {
      Serial.println("failed.");
    }
  } else if (c == 'V') {
    Serial.println("\nVerifing hex data in flash:");
    const char *data_ptr = bootloader_hex;
    for(;;) {
        byte type = char2byte(data_ptr + 7);
        if (pgm_read_byte(data_ptr) != ':') {
            Serial.println("\nERROR: Hex file format\n");
            break;
        }
        //printf("%c%c ",data_ptr[i*2 + 1], data_ptr[i*2 + 2]);
        //PutByte(type);
        if (type != 0) break;
        byte len = char2byte(data_ptr + 1);
        //PutByte(len);
        byte addr_hi = char2byte(data_ptr + 3);
        
        byte addr_lo = char2byte(data_ptr + 5);
        //printf("%02x ",addr_lo);
        data_ptr += 9;
        byte chksum = type + len + addr_hi + addr_lo;
        cmdPackage[0] = addr_lo;
        cmdPackage[1] = addr_hi;
        cmdPackage[2] = len;
        for(int i=0;i < len;i++) {
            cmdPackage[3+i] = char2byte(data_ptr);
            chksum += cmdPackage[3+i];
            //PutByte(cmdPackage[3+i]);
            data_ptr += 2;
        }
        chksum = 0x100 - chksum;
        if (char2byte(data_ptr) != chksum) {
            Serial.print("\nERROR: checksum fail: ");
            PutByte(char2byte(data_ptr));
            Serial.print(" != ");
            PutByte(chksum);
            Serial.println();
            break;
        }
        //Serial.println();
        cmdPackageLength = 3;
        cmdPackageGet = 0;
        C2_Connect_Target();
        C2_Read_Memory_to_buffer(C2_FP_READ_FLASH, verify_buffer);
        //Serial.print("W C2_RESPONSE: ");
        Serial.print("C2_RESPONSE: ");
        PutByte(C2_RESPONSE);
        Serial.println();
        PutByte(addr_hi);
        PutByte(addr_lo);
        Serial.print(" ");
        for (byte i=0; i<len; i++) {
          PutByte(verify_buffer[i]);
          if (verify_buffer[i] != cmdPackage[3+i]) {
            Serial.print("\nFail verify at byte ");
            PutByte(i);
            Serial.print(", Flash byte ");
            PutByte(verify_buffer[i]);
            Serial.print(" != ihex byte ");
            PutByte(cmdPackage[3+i]);
            Serial.println();
            break;
          }
        }
        Serial.println();
        data_ptr += 3;
    }
    Serial.println("\nVerifing ihex data in flash: complete\n");
  } else if (c == 'E') {
    Serial.println("\nErasing flash:\n");
    C2_Reset();
    C2_Erase_Flash_03();
    Serial.print("C2_RESPONSE: ");
    PutByte(C2_RESPONSE);
    Serial.println();
    Serial.println("\nErasing flash: complete\n");
  } else if (c == '4') {
    bootloader_hex = bootloader_hex_433;
    Serial.println("Changed the bootloader version to: 433");
  } else if (c == '8') {
    bootloader_hex = bootloader_hex_868;
    Serial.println("Changed the bootloader version to: 868");
  } else if (c == '9') {
    bootloader_hex = bootloader_hex_915;
    Serial.println("Changed the bootloader version to: 915");
  }
  
  if (C2D_PIN & C2D_BIT)
    LED_ON();
  else
    LED_OFF();
  delay(20);
}

uint8_t char2byte(char *ptr)
{
    byte b =char2hex(pgm_read_byte(ptr)) << 4;
    b |= char2hex(pgm_read_byte(ptr + 1));
    return b;
}

byte char2hex(char c)
{
  if (c >= '0' && c <= '9')
    return (c - '0');
  if (c >= 'A' && c <= 'F')
    return (c - 'A' + 0xA);
  if (c >= 'a' && c <= 'f')
    return (c - 'a' + 0xA);
  C2_RESPONSE = COMMAND_BAD_DATA;
  return 0;
}

byte ContainData_Q040(byte count)
{
  if ((cmdPackageLength - cmdPackageGet) == count)
    return 1;
  C2_RESPONSE = COMMAND_BAD_DATA;
  return 0;
}

byte GetNextByte()
{
  byte data = 0;
  if (cmdPackageGet < cmdPackageLength)
    data = cmdPackage[cmdPackageGet++];
  else
    C2_RESPONSE = COMMAND_BAD_DATA;
  return data;
}


/*******************************************************
 * 
 * Connect Target
 * Return:
 * 0x0D
 * 
 *******************************************************/
void C2_Connect_Target()
{
  C2_Reset();
  C2_WriteAR(C2ADD_FPCTL);
  if (C2_WriteDR(0x02) == COMMAND_OK)
  {
    if (1) //(C2_WriteDR(0x04) == COMMAND_OK)
    {
      delayMicroseconds(80);
      if (C2_WriteDR(0x01) == COMMAND_OK)
      {
        delay(25);

        C2_WriteAR(0x00);
        DeviceID = C2_ReadDR();

        Prepare_C2_Device_Params();
        Set_Connected_And_LEDs();
      }
    }
  }
}

void Prepare_C2_Device_Params()
{
  if (DeviceID == C8051F32X_DEVICEID || DeviceID == SI100X_DEVICEID)
    C2ADD_FPDAT = 0xB4;
  else
    C2ADD_FPDAT = 0xAD;
}

void Set_Connected_And_LEDs()
{
  C2_CONNECTED = 1;
  LEDS_STATE = 2;
}

/*******************************************************
 * 
 * Disconnect Target
 * Return:
 * 0x0D
 * 
 *******************************************************/
byte C2_Disconnect_Target()
{
  C2_WriteAR(C2ADD_FPCTL);
  C2_WriteDR(0x00);
  delayMicroseconds(40);
  C2_Reset();
  C2_CONNECTED = 0;
  LEDS_STATE = 0;
  return COMMAND_OK;
}

/*******************************************************
 * 
 * Get Device ID and Revision
 * Return:
 *  DeviceID
 *  Revision
 *  0x0D
 * 
 *******************************************************/
void C2_Device_ID()
{
  C2_WriteAR(C2ADD_DEVICEID);
  PutByte(C2_ReadDR());

  C2_WriteAR(C2ADD_REVISION);
  PutByte(C2_ReadDR());
}

/*******************************************************
 * 
 * Get Unique Device ID
 * Return:
 *  Unique Device ID 1
 *  Unique Device ID 1
 *  0x0D
 * 
 *******************************************************/
void C2_Unique_Device_ID()
{
  byte id1 = 0;
  byte id2 = 0;
  C2_RESPONSE = COMMAND_NO_CONNECT;
  if (C2_CONNECTED)
  {
    if (C2_Write_FPDAT_Read(0x01) == COMMAND_OK)
    {
      id1 = C2_Read_FPDAT();
      if (C2_RESPONSE == COMMAND_OK)
      {
        if (C2_Write_FPDAT_Read(0x02) == COMMAND_OK)
          id2 = C2_Read_FPDAT();
      }
    }
  }
  PutByte(id1);
  PutByte(id2);
}

/*******************************************************
 * 
 * Target Go
 * Return:
 *  1   not connected
 *  0x0D go
 * 
 *******************************************************/
void C2_Target_Go()
{
  C2_RESPONSE = COMMAND_OK;
  if (C2_CONNECTED)
  {
    C2_WriteAR(C2ADD_FPCTL);
    if (C2_WriteDR(0x08) == COMMAND_OK)
    {
      C2CK_DriverOff();
      C2_CONNECTED = 0;
      LEDS_STATE = 1;
    }
  }
}

/*******************************************************
 * 
 * Target Halt
 * Return:
 *  0x0D
 * 
 *******************************************************/
void C2_Target_Halt()
{
  C2_RESPONSE = COMMAND_OK;
  if (!C2_CONNECTED)
  {
    if (C2CK_PIN & C2CK_BIT)
    {
      cli();
      C2CK_DriverOn();
      C2CK_PORT &= ~C2CK_BIT;
      delayMicroseconds(1);
      C2CK_DriverOff();
      sei();
    }
    unsigned long timeout = millis();
    while (!(C2CK_PIN & C2CK_BIT))
    {
      if (millis() - timeout > 1000)
      {
        C2_RESPONSE = COMMAND_FAILED;
        return;
      }
    }
    C2CK_DriverOn();
    Set_Connected_And_LEDs();
  }
}

/*******************************************************
 * 
 * Erase Entire Flash (command 03)
 * Return:
 *   0x0D
 * 
 *******************************************************/
void C2_Erase_Flash_03()
{
  byte data;

  C2_RESPONSE = COMMAND_NO_CONNECT;
  if (!C2_CONNECTED)
    return;
/* Set VDD monitor. Seems not needed for si1000.
  C2_WriteAR(0xFF);
  C2_WriteDR(0x80);
  C2_WriteAR(0xEF);
  C2_WriteDR(0x02);*/

  C2_WriteAR(C2ADD_FPCTL);
  C2_WriteDR(0x02);
  C2_WriteDR(0x01);

  delay(20);

  while (1)
  {
    C2_WriteAR(C2ADD_FPDAT);

    if (C2_WriteDR(0x03) != COMMAND_OK)
    {
      PutByte(0x51);
      break;
    }
    if (Poll_InBusy() != COMMAND_OK)
    {
      PutByte(0x52);
      break;
    }
    if (Poll_OutReady() != COMMAND_OK)
    {
      PutByte(0x53);
      break;
    }
    data = C2_ReadDR();
    if (C2_RESPONSE != COMMAND_OK)
    {
      PutByte(0x54);
      break;
    }
//    if (data != COMMAND_OK)
//    {
//      PutByte(0x55);
//      PutByte(data);
//      break;
//    }

    if (C2_WriteDR(0xDE) != COMMAND_OK)
    {
      PutByte(0x56);
      break;
    }
    if (Poll_InBusy() != COMMAND_OK)
    {
      PutByte(0x57);
      break;
    }

    if (C2_WriteDR(0xAD) != COMMAND_OK)
    {
      PutByte(0x58);
      break;
    }
    if (Poll_InBusy() != COMMAND_OK)
    {
      PutByte(0x59);
      break;
    }

    if (C2_WriteDR(0xA5) != COMMAND_OK)
    {
      PutByte(0x5A);
      break;
    }
    if (Poll_InBusy() != COMMAND_OK)
    {
      PutByte(0x5B);
      break;
    }

    if (Poll_OutReady() != COMMAND_OK)
    {
      PutByte(0x5C);
      break;
    }

    data = C2_ReadDR();
    if (C2_RESPONSE != COMMAND_OK)
    {
      PutByte(0x5D);
      break;
    }
    if (data != COMMAND_OK)
    {
      PutByte(0x5E);
      PutByte(data);
      break;
    }

    break;
  }
}

/*******************************************************
 * 
 * Erase Entire Flash (command 04)
 * Return:
 *   0x0D
 * 
 *******************************************************/
void C2_Erase_Flash_04()
{
  C2_RESPONSE = COMMAND_NO_CONNECT;
  byte data;
  if (C2_CONNECTED)
    if ((data = C2_Write_FPDAT_Read(0x04)) != COMMAND_OK)
      C2_RESPONSE = data;
    else if ((data = C2_Write_FPDAT(0xDE)) != COMMAND_OK)
      C2_RESPONSE = data;
    else if ((data = C2_Write_FPDAT(0xAD)) != COMMAND_OK)
      C2_RESPONSE = data;
    else if ((data = C2_Write_FPDAT(0xA5)) != COMMAND_OK)
      C2_RESPONSE = data;
    else
      C2_RESPONSE = C2_Read_FPDAT();
}

/*******************************************************
 * 
 * Get Firmware Version
 * Return:
 *  version number
 * 
 *******************************************************/
void Get_Firmware_Version()
{
  PutByte(0x44);
  PutByte(0x06);
  C2_RESPONSE = COMMAND_OK;
}

/*******************************************************
 * 
 * Set FPDAT register address
 * 
 *******************************************************/
void Set_FPDAT_Address()
{
  C2ADD_FPDAT = GetNextByte();
  if (C2_RESPONSE == COMMAND_BAD_DATA)
    return;
  C2_RESPONSE = COMMAND_OK;
}

/*******************************************************
 * 
 * Get FPDAT register address
 * 
 *******************************************************/
void Get_FPDAT_Address()
{
  PutByte(C2ADD_FPDAT);
  C2_RESPONSE = COMMAND_OK;
}

/*******************************************************
 * 
 * Read Memory
 *  06, read flash
 *  09, read SFR
 *  0B, read RAM
 *  0E, read XRAM
 * Return:
 *  0xD
 * 
 *******************************************************/
void C2_Read_Memory(byte memType)
{
  byte address16bit = (memType == C2_FP_READ_XRAM || memType == C2_FP_READ_FLASH ? 1 : 0);
  byte lowAddress = GetNextByte();
  byte highAddress = 0;
  if (address16bit)
    highAddress = GetNextByte();
  byte byteCount = GetNextByte();
  byte data;

  if (C2_RESPONSE == COMMAND_BAD_DATA)
    return;

  C2_RESPONSE = COMMAND_NO_CONNECT;
  for (;;)
  {
    if (!C2_CONNECTED)
      break;

    // Write FP Command
    data = C2_Write_FPDAT_Read(memType);
    if (data != COMMAND_OK)
    {
      PutByte(data);
      C2_RESPONSE = COMMAND_READ_01;
      break;
    }

    // Write high address for Flash and XRAM
    if (address16bit)
    {
      data = C2_Write_FPDAT(highAddress);
      if (data != COMMAND_OK)
      {
        PutByte(data);
        C2_RESPONSE = COMMAND_READ_02;
        break;
      }
    }

    // Write low address
    data = C2_Write_FPDAT(lowAddress);
    if (data != COMMAND_OK)
    {
      PutByte(data);
      C2_RESPONSE = COMMAND_READ_03;
      break;
    }

    // Write byte count
    data = C2_Write_FPDAT(byteCount);
    if (data != COMMAND_OK)
    {
      PutByte(data);
      C2_RESPONSE = COMMAND_READ_04;
      break;
    }

    // Read response only for Flash
    if (memType == C2_FP_READ_FLASH)
    {
      C2_RESPONSE = C2_Read_FPDAT();
      if (C2_RESPONSE != COMMAND_OK)
        break;
    }

    // Read data to host
    do //for (; byteCount; byteCount--)
    {
      data = C2_Read_FPDAT();
      if (C2_RESPONSE != COMMAND_OK)
        break;
      PutByte(data);
      byteCount -= 1;
    }  while(byteCount);
    break;
  }
}


/*******************************************************
 * 
 * Read Memory
 *  06, read flash
 *  09, read SFR
 *  0B, read RAM
 *  0E, read XRAM
 * Return:
 *  0xD
 * 
 *******************************************************/
void C2_Read_Memory_to_buffer(byte memType, byte *ptr)
{
  byte address16bit = (memType == C2_FP_READ_XRAM || memType == C2_FP_READ_FLASH ? 1 : 0);
  byte lowAddress = GetNextByte();
  byte highAddress = 0;
  if (address16bit)
    highAddress = GetNextByte();
  byte byteCount = GetNextByte();
  byte data;

  if (C2_RESPONSE == COMMAND_BAD_DATA)
    return;

  C2_RESPONSE = COMMAND_NO_CONNECT;
  for (;;)
  {
    if (!C2_CONNECTED)
      break;

    // Write FP Command
    data = C2_Write_FPDAT_Read(memType);
    if (data != COMMAND_OK)
    {
      PutByte(data);
      C2_RESPONSE = COMMAND_READ_01;
      break;
    }

    // Write high address for Flash and XRAM
    if (address16bit)
    {
      data = C2_Write_FPDAT(highAddress);
      if (data != COMMAND_OK)
      {
        PutByte(data);
        C2_RESPONSE = COMMAND_READ_02;
        break;
      }
    }

    // Write low address
    data = C2_Write_FPDAT(lowAddress);
    if (data != COMMAND_OK)
    {
      PutByte(data);
      C2_RESPONSE = COMMAND_READ_03;
      break;
    }

    // Write byte count
    data = C2_Write_FPDAT(byteCount);
    if (data != COMMAND_OK)
    {
      PutByte(data);
      C2_RESPONSE = COMMAND_READ_04;
      break;
    }

    // Read response only for Flash
    if (memType == C2_FP_READ_FLASH)
    {
      C2_RESPONSE = C2_Read_FPDAT();
      if (C2_RESPONSE != COMMAND_OK)
        break;
    }

    // Read data to host
    do //for (; byteCount; byteCount--)
    {
      data = C2_Read_FPDAT();
      if (C2_RESPONSE != COMMAND_OK)
        break;
      *ptr = data;
      ptr++;
      byteCount -= 1;
    }  while(byteCount);
    break;
  }
}

/*******************************************************
 * 
 * Write Memory
 *  07, write to flash
 *  0A, write to SFR
 *  0C, write to RAM
 *  0F, write to XRAM
 * Return:
 *  0xD
 * 
 *******************************************************/
void C2_Write_Memory(byte memType)
{
  byte address16bit = (memType == C2_FP_WRITE_XRAM || memType == C2_FP_WRITE_FLASH ? 1 : 0);
  byte lowAddress = GetNextByte();
  byte highAddress = 0;
  if (address16bit)
    highAddress = GetNextByte();
  byte byteCount = GetNextByte();
  //Serial.print("byteCount:");
  //PutByte(byteCount);
  //Serial.println();
  if ((C2_RESPONSE == COMMAND_BAD_DATA) || !ContainData_Q040(byteCount))
    return;

  C2_RESPONSE = COMMAND_NO_CONNECT;
  for (;;)
  {
    if (!C2_CONNECTED)
      break;

    if (C2_Write_FPDAT_Read(memType) != COMMAND_OK)
      break;

    if (address16bit)
      if (C2_Write_FPDAT(highAddress) != COMMAND_OK)
        break;

    if (C2_Write_FPDAT(lowAddress) != COMMAND_OK)
      break;

    if (C2_Write_FPDAT(byteCount) != COMMAND_OK)
      break;

    if (memType == C2_FP_WRITE_FLASH)
      if (C2_Read_FPDAT() != COMMAND_OK)
        break;

    for (; byteCount; byteCount--)
      if (C2_Write_FPDAT(GetNextByte()) != COMMAND_OK)
        break;

    if (C2_RESPONSE == COMMAND_OK)
      Poll_OutReady();

    break;
  }
}

/*******************************************************
 * 
 * Erase Flash Sector
 * Return:
 *  0xD
 * 
 *******************************************************/
void C2_Erase_Flash_Sector(void)
{
  byte sector = GetNextByte();
  if (C2_RESPONSE == COMMAND_BAD_DATA)
    return;

  C2_RESPONSE = COMMAND_NO_CONNECT;
  for (;;)
  {
    if (!C2_CONNECTED)
      break;
    if (C2_Write_FPDAT_Read(C2_FP_ERASE_SECTOR) != COMMAND_OK)
      break;
    if (C2_Write_FPDAT_Read(sector) != COMMAND_OK)
      break;
    C2_Write_FPDAT(0);
    break;
  }
}

/*******************************************************
 * 
 * C2 protocol helpers
 * 
 *******************************************************/

//
// Make RESET
//
//   C2CK ----\              /----------\         /
//            |\            /|          |\       /
//            | \----------/ |          | \-----/
//     driver |              |          |
//       on ->+<--- t_RD --->+<- t_SD -+
//                  >20us        >2us
//
// To generate the reset timing:
//   1. Turn the C2CK driver on.
//   2. Force C2CK low.
//   3. Wait at least 20 μs.
//   4. Force C2CK high.
//   5. Wait at least 2 μs.
//   6. (Optional) Turn the C2CK driver off.
//
byte C2_Reset(void)
{
  cli();
  C2CK_DriverOn();
  C2CK_PORT &= ~C2CK_BIT;
  delayMicroseconds(20);
  C2CK_PORT |= C2CK_BIT;
  delayMicroseconds(2);
  sei();
  C2_RESPONSE = COMMAND_OK;
}

//
// Make pulse on C2CLK line
//
//   C2CK ---+--------\          /-------------\         /
//           |        |\        /|             |\       /
//           |        | \------/ |             | \-----/
//    driver |        |          |             |
//      on ->+<------>+<- t_CL ->+<-- t_CH --->+
//             >40ns   80ns - 5us    >120ns
//
// To generate C2CK clock strobes with a microcontroller-based programmer:
//   1. Turn the C2CK driver on.
//   2. Wait at least 40 ns. This helps ensure C2D data setup time.
//   3. Force C2CK low. Ensure interrupts are disabled at this point.
//   4. Wait between 80 and 5000 ns.
//   5. Force C2CK high.
//   6. Wait at least 120 ns. This helps ensure C2D data valid time.
//
void Pulse_C2CLK(void)
{
  C2CK_PORT &= ~C2CK_BIT;
  asm volatile("nop\n\t");
  asm volatile("nop\n\t");
  C2CK_PORT |= C2CK_BIT;
  asm volatile("nop\n\t");
  asm volatile("nop\n\t");
}

//
// Drive On/Off C2D line
//
void C2D_DriverOn()
{
  C2D_DDR |= C2D_BIT;
}

void C2D_DriverOff()
{
  C2D_PORT &= ~C2D_BIT;
  C2D_DDR  &= ~C2D_BIT;
}

//
// Drive On/Off C2CLK line
//
void C2CK_DriverOn()
{
  C2CK_PORT |= C2CK_BIT;
  C2CK_DDR  |= C2CK_BIT;
}

void C2CK_DriverOff()
{
  C2CK_PORT |=  C2CK_BIT;
  C2CK_DDR  &= ~C2CK_BIT;
}

//
// Write to FPDAT register
//
byte C2_Write_FPDAT_Read(byte data)
{
  if (C2_Write_FPDAT(data) == COMMAND_OK)
    return C2_Read_FPDAT();
  return 0;
}

//
// Wait for set OutReady
//
byte Poll_InBusy()
{
  C2_RESPONSE = COMMAND_OK;
  unsigned int timeout = 50000;
  byte h_timeout = 0;
  while ((C2_ReadAR() & C2ADD_INBUSY))
  {
    if (!h_timeout)
    {
      h_timeout = 50;
      if (--timeout == 0)
      {
        C2_RESPONSE = COMMAND_TIMEOUT;
        break;
      }
    }
    --h_timeout;
    delayMicroseconds(1);
  }
  return C2_RESPONSE;
}

//
// Wait for set OutReady
//
byte Poll_OutReady()
{
  C2_RESPONSE = COMMAND_OK;
  unsigned int timeout = 50000;
  byte h_timeout = 0;
  while (!(C2_ReadAR() & C2ADD_OUTREADY))
  {
    if (!h_timeout)
    {
      h_timeout = 50;
      if (--timeout == 0)
      {
        C2_RESPONSE = COMMAND_TIMEOUT;
        break;
      }
    }
    --h_timeout;
    delayMicroseconds(1);
  }
  return C2_RESPONSE;
}

//
// Read from FPDAT
//
byte C2_Read_FPDAT()
{
  if (Poll_OutReady() == COMMAND_OK)
  {
    C2_WriteAR(C2ADD_FPDAT);
    return C2_ReadDR();
  }
  return 0;
}

//
// Write to FPDAT with wait InBusy clear
//
byte C2_Write_FPDAT(byte data)
{
  C2_WriteAR(C2ADD_FPDAT);

  if (C2_WriteDR(data) == COMMAND_OK)
  {
    unsigned int timeout = 50000;
    byte h_timeout = 0;
    while (C2_ReadAR() & C2ADD_INBUSY)
    {
      if (!h_timeout)
      {
        h_timeout = 50;
        if (--timeout == 0)
        {
          C2_RESPONSE = COMMAND_TIMEOUT;
          break;
        }
      }
      --h_timeout;
      delayMicroseconds(1);
    }
  }
  return C2_RESPONSE;
}

//-----------------------------------------------------------------------------------
// C2_ReadAR()
//-----------------------------------------------------------------------------------
// - Performs a C2 Address register read
// - Returns the 8-bit register content
//
byte C2_ReadAR()
{
  cli();
  // START field
  Pulse_C2CLK();

  // INS field (10b, LSB first)
  C2D_PORT &= ~C2D_BIT;
  C2D_DriverOn();
  Pulse_C2CLK();
  C2D_PORT |= C2D_BIT;
  Pulse_C2CLK();
  C2D_DriverOff();

  // ADDRESS field
  byte data = 0;
  for (byte i = 0; i < 8; i++)
  {
    Pulse_C2CLK();
    data = data >> 1;
    data &= 0x7F;
    if (C2D_PIN & C2D_BIT)
      data |= 0x80;
  }

  // STOP field
  Pulse_C2CLK();
  sei();

  C2_RESPONSE = COMMAND_OK;
  return data;
}

//-----------------------------------------------------------------------------------
// C2_WriteAR()
//-----------------------------------------------------------------------------------
// - Performs a C2 Address register write (writes the <addr> input 
//   to Address register)
//
void C2_WriteAR(byte addr)
{
  cli();
  // START field
  Pulse_C2CLK();

  // INS field (11b, LSB first)
  C2D_PORT |= C2D_BIT;
  C2D_DriverOn();
  Pulse_C2CLK();
  Pulse_C2CLK();

  // ADDRESS field
  for (byte i = 0; i < 8; i++)
  {
    if (addr & 0x01)
      C2D_PORT |= C2D_BIT;
    else
      C2D_PORT &= ~C2D_BIT;
    addr = addr >> 1;
    Pulse_C2CLK();
  }

  // STOP field
  C2D_DriverOff();
  Pulse_C2CLK();
  sei();

  C2_RESPONSE = COMMAND_OK;
  return;
}

//-----------------------------------------------------------------------------------
// C2_ReadDR()
//-----------------------------------------------------------------------------------
// - Performs a C2 Data register read
// - Returns the 8-bit register content
//
byte C2_ReadDR()
{
  cli();
  // START field
  Pulse_C2CLK();

  C2D_PORT &= ~C2D_BIT;
  C2D_DriverOn();

  Pulse_C2CLK();    // INS field (00b, LSB first)
  Pulse_C2CLK();

  Pulse_C2CLK();    // LENGTH field (00b -> 1 byte)
  Pulse_C2CLK();

  // WAIT field
  C2D_DriverOff();  // Disable C2D driver for input
  byte retry = 0;
  byte data = 0;
  do
    Pulse_C2CLK();
  while (--retry && !(C2D_PIN & C2D_BIT));

  if (retry)
  {
    // DATA field
    for (byte i = 0; i < 8; i++)
    {
      Pulse_C2CLK();
      data = data >> 1;
      if (C2D_PIN & C2D_BIT)
        data |= 0x80;
      else
        data &= 0x7F;
    }

    // STOP field
    Pulse_C2CLK();
  }

  sei();

  C2_RESPONSE = (retry ? COMMAND_OK : COMMAND_TIMEOUT);
  return data;
}

//-----------------------------------------------------------------------------------
// C2_WriteDR()
//-----------------------------------------------------------------------------------
// - Performs a C2 Data register write (writes <data> input to data register)
//
byte C2_WriteDR(byte data)
{
  cli();
  // START field
  Pulse_C2CLK();

  // INS field (01b, LSB first)
  C2D_PORT |= C2D_BIT;
  C2D_DriverOn();
  Pulse_C2CLK();
  C2D_PORT &= ~C2D_BIT;
  Pulse_C2CLK();

  // LENGTH field (00b -> 1 byte)
  Pulse_C2CLK();
  Pulse_C2CLK();

  // DATA field
  for (byte i = 0; i < 8; i++)
  {
    if (data & 0x01)
      C2D_PORT |= C2D_BIT;
    else
      C2D_PORT &= ~C2D_BIT;
    data = data >> 1;
    Pulse_C2CLK();
  }

  C2D_DriverOff();
  byte retry = 200;
  do
    Pulse_C2CLK();
  while (--retry && !(C2D_PIN & C2D_BIT));

  // STOP field
  Pulse_C2CLK();

  sei();

  C2_RESPONSE = (retry ? COMMAND_OK : COMMAND_TIMEOUT);
  return C2_RESPONSE;
}

//
// Blink LED
//
void BlinkLED(byte flashes)
{
  delay(100);
  while (flashes-- > 0)
  {
    LED_ON();
    delay(100);
    LED_OFF();
    delay(250);
  }
}

char hex2char(byte data)
{
  if (data >= 0x0A)
    Serial.write(data + ('A' - 0x0A));
  else
    Serial.write(data + '0');
}

void PutByte(byte data)
{
  hex2char((data >> 4) & 0x0F);
  hex2char(data & 0x0F);
}
